/**
 * Testing example code for the Inline current sensing class
*/
#include <SimpleFOC.h>
#include "Arduino.h"
// current sensor
// shunt resistor value //单位是什么？欧姆吗？采样电阻是0.01欧姆？
// gain value
// pins phase A,B, (C optional) //这个是干嘛的？得到ADC值不是只要1相就可以了吗

InlineCurrentSense M0_current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A1);
InlineCurrentSense M1_current_sense = InlineCurrentSense(0.01f, 50.0f, A2, A3);


void setup() {

  // use monitoring with serial 
  Serial.begin(115200);
  // enable more verbose output for debugging
  // comment out if not needed
  SimpleFOCDebug::enable(&Serial);

  // initialise the current sensing
  if(!M0_current_sense.init()){
    Serial.println("M0 Current sense init failed.");
    return;
  }


  if(!M1_current_sense.init()){
    Serial.println("M1 Current sense init failed.");
    return;
  }

  // for SimpleFOCShield v2.01/v2.0.2
  //current_sense.gain_b *= -1; //为什么要这样写？
  
  Serial.println("Current sense ready.");
}



void myprintf(const char* format, ...) {
    char buffer[256];
    va_list args;
    va_start(args, format);
    vsnprintf(buffer, sizeof(buffer), format, args);
    va_end(args);
    Serial.print(buffer);
}

void loop() {

    PhaseCurrent_s M0_currents = M0_current_sense.getPhaseCurrents();//这样直接就能得到具体的电压了吗
    PhaseCurrent_s M1_currents = M1_current_sense.getPhaseCurrents();//这样直接就能得到具体的电压了吗
    //float current_magnitude = current_sense.getDCCurrent();//什么叫 current_magnitude，magnitude是什么意思？

    #if  0
    Serial.print(currents.a*1000); // milli Amps
    Serial.print("\t");
    Serial.print(currents.b*1000); // milli Amps
    Serial.print("\t");
    Serial.print(currents.c*1000); // milli Amps
    Serial.print("\t");
    Serial.println(current_magnitude*1000); // milli Amps
    #endif 

    //修改为支持VOFA+
    //Serial.printf("%d,%d,%d\n",currents.a*1000, currents.b*1000,  currents.c*1000); //ARDUINO 默认不支持
    //myprintf("%d,%d,%d\n",currents.a*1000, currents.b*1000,  currents.c*1000); 
    
    #if 1
    Serial.print(M0_currents.a * 1000); Serial.print(",");
    Serial.print(M0_currents.b * 1000); Serial.print(",");
    //Serial.print(M0_currents.c * 1000); Serial.println();
    #endif

    #if 1
    Serial.print(M1_currents.a * 1000); Serial.print(",");
    Serial.print(M1_currents.b * 1000);  Serial.println();
    //Serial.print(M1_currents.c * 1000); Serial.println();
   
    #endif
  }